/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "robotattribute.h"
//#include "../../controllercommondefine.h"
#include "MacroDefine/GeneralDefine.h"
#include <QVariant>
#include <QSettings>
#include "md5check.h"

RobotAttribute::RobotAttribute(QString fileNameIn , int robotIdIn, MotionMessage *motionMessageIn)
{
    motionMessage=motionMessageIn;
    filePath=fileNameIn;
    robotId=robotIdIn;
    isUsePositionRegulatorStatus=false;

    deltaTime=0.001;
    coordinateSize=100;


}

int RobotAttribute::setGravityVector(vector<double> gravityIn)
{
    if(3!=gravityIn.size())
    {
        qDebug()<<"error,setGravityVector 3!=gravityIn.size()";
        return 0;
    }
    bool ok = openXml(filePath, QIODevice::ReadWrite );
    if( !ok )//无法打开机器人数据文件！
    {
        return -1;
    }

    QDomNode  typeNode = findSubNode(m_root, "robotSetting", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotSetting"<<filePath;
        }
        return -1;
    }



    writeXml( findSubNode(typeNode, "gravity_x", ok), gravityIn[0]);
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取gravity_x失败";
        }
        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5225, robotId);
        return -1;
    }

    writeXml( findSubNode(typeNode, "gravity_y", ok), gravityIn[1]);
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取gravity_y失败";
        }
        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5225, robotId);
        return -1;
    }

    writeXml( findSubNode(typeNode, "gravity_z", ok), gravityIn[2]);
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取gravity_z失败";
        }
        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5225, robotId);
        return -1;
    }

     closeXml(true);

     return 1;
}



int RobotAttribute::setVehicleTrajectoryControlParameter(VehicleTrajectoryControlParameter infoIn, int &returnFlag)
{
    returnFlag=0;
    //unit change to mm
    infoIn.maxAcc_line=infoIn.maxAcc_line*1000.0;
    infoIn.maxAcc_rotate=infoIn.maxAcc_rotate*1000.0;
    infoIn.maxVel_line=infoIn.maxVel_line*1000.0;
    infoIn.maxVel_rotate=infoIn.maxVel_rotate*1000.0;

    bool ok = openXml(filePath, QIODevice::ReadWrite );
    if( !ok )//无法打开机器人数据文件！
    {
        return -1;
    }

    QDomNode  typeNode = findSubNode(m_root, "robotSetting", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotSetting"<<filePath;
        }
        return -1;
    }


    writeXml( findSubNode(typeNode, "agv_lineMaxSpeed", ok), infoIn.maxVel_line );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_lineMaxSpeed";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5207, robotId);
        return -1;
    }

    writeXml( findSubNode(typeNode, "agv_rotateMaxSpeed", ok), infoIn.maxVel_rotate  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_rotateMaxSpeed";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5207, robotId);
        return -1;
    }



    writeXml( findSubNode(typeNode, "agv_lineMaxAcceleration", ok), infoIn.maxAcc_line  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_lineMaxAcceleration";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5208, robotId);
        return -1;
    }


    writeXml( findSubNode(typeNode, "agv_rotateMaxAcceleration", ok), infoIn.maxAcc_rotate );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_rotateMaxAcceleration";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5208, robotId);
        return -1;
    }

     closeXml(true);
     returnFlag=1;
     return 1;
}

int RobotAttribute::setAxisPositionLimit(int axisId, double minPositionIn, double maxPositionIn,
                                         double approachDistanceIn, int &returnFlag)
{
    returnFlag=0;

    bool ok = openXml(filePath, QIODevice::ReadWrite );
    if( !ok )//无法打开机器人数据文件！
    {
        return -1;
    }

    QDomNode  typeNode = findSubNode(m_root, "robotAxis", ok );
    if( !ok )
    {
        qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotSetting"<<filePath;
        return -1;
    }

    QDomNode  tmpNode =  findSubNode(typeNode, QString("axis%1").arg(axisId), ok);
    if( !ok )
    {
        qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotSetting"<<filePath;
        return -1;
    }

    writeXml( findSubNode(tmpNode, "minPosition", ok), minPositionIn );
    if( !ok)
    {
        return -1;
    }

    writeXml( findSubNode(tmpNode, "maxPositionIn", ok), maxPositionIn );
    if( !ok)
    {
        return -2;
    }


    writeXml( findSubNode(tmpNode, "approachDistance", ok), approachDistanceIn );
    if( !ok)
    {
        return -3;
    }

     closeXml(true);
     returnFlag=1;
     return 1;
}

int RobotAttribute::setAxisProperty(int axisId, AxisProperty_hmi axisPropertyIn)
{

    int returnFlag=0;

    bool ok = openXml(filePath, QIODevice::ReadWrite );
    if( !ok )//无法打开机器人数据文件！
    {
        return -1;
    }

    QDomNode  typeNode = findSubNode(m_root, "robotAxis", ok );
    if( !ok )
    {
        qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotSetting"<<filePath;
        return -2;
    }

    QDomNode  tmpNode =  findSubNode(typeNode, QString("axis%1").arg(axisId), ok);
    if( !ok )
    {
        qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotSetting"<<filePath;
        return -3;
    }

    writeXml( findSubNode(tmpNode, "minPosition", ok), axisPropertyIn.minPosition );
    if( !ok)
    {
        return -4;
    }

    writeXml( findSubNode(tmpNode, "maxPosition", ok), axisPropertyIn.maxPosition );
    if( !ok)
    {
        return -5;
    }

    writeXml( findSubNode(tmpNode, "maxVelocity", ok), axisPropertyIn.maxVelocity );
    if( !ok)
    {
        return -6;
    }
    writeXml( findSubNode(tmpNode, "maxFeedbackVelocity", ok), axisPropertyIn.maxFeedbackVelocity );
    if( !ok)
    {
        return -7;
    }
    writeXml( findSubNode(tmpNode, "approachDistance", ok), axisPropertyIn.approachDistance );
    if( !ok)
    {
        return -8;
    }
    writeXml( findSubNode(tmpNode, "maxAccelation", ok), axisPropertyIn.maxAccelation );
    if( !ok)
    {
        return -9;
    }

    writeXml( findSubNode(tmpNode, "processVelocityFollowError", ok), axisPropertyIn.processVelocityFollowError );
    if( !ok)
    {
        return -10;
    }
    writeXml( findSubNode(tmpNode, "processFollowError", ok), axisPropertyIn.processFollowError );
    if( !ok)
    {
        return -11;
    }
    writeXml( findSubNode(tmpNode, "targetFollowError", ok), axisPropertyIn.targetFollowError );
    if( !ok)
    {
        return -12;
    }
    writeXml( findSubNode(tmpNode, "jogTorqueLimit", ok), axisPropertyIn.jogTorqueLimit );
    if( !ok)
    {
        return -13;
    }
    writeXml( findSubNode(tmpNode, "motorRatedTorque", ok), axisPropertyIn.motorRatedTorque );
    if( !ok)
    {
        return -14;
    }
    writeXml( findSubNode(tmpNode, "motorTorqueLimitPositive", ok), axisPropertyIn.motorTorqueLimitPositive );
    if( !ok)
    {
        return -15;
    }
    writeXml( findSubNode(tmpNode, "motorTorqueLimitNegative", ok), axisPropertyIn.motorTorqueLimitNegative );
    if( !ok)
    {
        return -16;
    }
    writeXml( findSubNode(tmpNode, "maxTorque", ok), axisPropertyIn.maxTorque );
    if( !ok)
    {
        return -17;
    }

     closeXml(true);
     returnFlag=1;
     robotJoints[axisId].minPosition=axisPropertyIn.minPosition;
     robotJoints[axisId].maxPosition=axisPropertyIn.maxPosition;
     robotJoints[axisId].approachDistance=axisPropertyIn.approachDistance;
     robotJoints[axisId].maxVelocity=axisPropertyIn.maxVelocity;
     robotJoints[axisId].maxFeedbackVelocity=axisPropertyIn.maxFeedbackVelocity;
     robotJoints[axisId].maxAccelation=axisPropertyIn.maxAccelation;
     robotJoints[axisId].processVelocityFollowError=axisPropertyIn.processVelocityFollowError;
     robotJoints[axisId].processFollowError=axisPropertyIn.processFollowError;
     robotJoints[axisId].targetFollowError=axisPropertyIn.targetFollowError;
     robotJoints[axisId].jogTorqueLimit=axisPropertyIn.jogTorqueLimit;
     robotJoints[axisId].motorRatedTorque=axisPropertyIn.motorRatedTorque;
     robotJoints[axisId].motorTorqueLimitPositive=axisPropertyIn.motorTorqueLimitPositive;
     robotJoints[axisId].motorTorqueLimitNegative=axisPropertyIn.motorTorqueLimitNegative;
     robotJoints[axisId].maxTorque=axisPropertyIn.maxTorque;
     return 1;
}

int RobotAttribute::getAxisProperty(int axisId, AxisProperty_hmi &axisPropertyOut)
{
    if(axisId>=0 && axisId<robotJoints.size())
    {
        axisPropertyOut.minPosition=robotJoints[axisId].minPosition;
        axisPropertyOut.maxPosition=robotJoints[axisId].maxPosition;
        axisPropertyOut.approachDistance=robotJoints[axisId].approachDistance;
        axisPropertyOut.maxVelocity=robotJoints[axisId].maxVelocity;
        axisPropertyOut.maxFeedbackVelocity=robotJoints[axisId].maxFeedbackVelocity;
        axisPropertyOut.maxAccelation=robotJoints[axisId].maxAccelation;
        axisPropertyOut.processVelocityFollowError=robotJoints[axisId].processVelocityFollowError;
        axisPropertyOut.processFollowError=robotJoints[axisId].processFollowError;
        axisPropertyOut.targetFollowError=robotJoints[axisId].targetFollowError;
        axisPropertyOut.jogTorqueLimit=robotJoints[axisId].jogTorqueLimit;
        axisPropertyOut.motorRatedTorque=robotJoints[axisId].motorRatedTorque;
        axisPropertyOut.motorTorqueLimitPositive=robotJoints[axisId].motorTorqueLimitPositive;
        axisPropertyOut.motorTorqueLimitNegative=robotJoints[axisId].motorTorqueLimitNegative;
        axisPropertyOut.maxTorque=robotJoints[axisId].maxTorque;
        return 1;
    }

    return -1;
}

int RobotAttribute::getRobotCoordinateConfig(RobotCoordinateAxisConfig &coordinateConfigOut)
{
    coordinateConfigOut.arm_CeaseTime=arm_CeaseTime;
    coordinateConfigOut.arm_StopTime=arm_StopTime;
    coordinateConfigOut.lineMaxAcceleration=lineMaxAcceleration;
    coordinateConfigOut.lineMaxSpeed=lineMaxSpeed;
    coordinateConfigOut.rotate_eqradius=rotate_eqradius;
    coordinateConfigOut.t1RatioLimit=t1RatioLimit;
    coordinateConfigOut.targetWaitTime=targetWaitTime;
    coordinateConfigOut.jogDecartLineRatio=jogDecartLineRatio;
    coordinateConfigOut.jogDecartRotateRation=jogDecartRotateRation;
    return 1;
}

int RobotAttribute::setRobotCoordinateConfig(RobotCoordinateAxisConfig coordinateConfigIn)
{
    bool ok = openXml(filePath, QIODevice::ReadWrite );
    if( !ok )//无法打开机器人数据文件！
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====无法打开机器人参数文件！";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5200, robotId);

        return -1;
    }


    QDomNode  typeNode = findSubNode(m_root, "robotSetting", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotSetting"<<filePath;
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5201, robotId);
        return -1;
    }


    writeXml( findSubNode(typeNode, "t1RatioLimit", ok), coordinateConfigIn.t1RatioLimit  );
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取T1限速失败，使用默认值";
        }
        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5204, robotId);
        return -1;
    }


    writeXml( findSubNode(typeNode, "arm_CeaseTime", ok), coordinateConfigIn.arm_CeaseTime  );
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取arm_CeaseTime失败";
        }
        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5223, robotId);

    }

    writeXml( findSubNode(typeNode, "arm_StopTime", ok), coordinateConfigIn.arm_StopTime  );
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取arm_StopTime失败";
        }
        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5224, robotId);

    }

    writeXml( findSubNode(typeNode, "targetWaitTime", ok), coordinateConfigIn.targetWaitTime  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: targetWaitTime";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5206, robotId);
        return -1;
    }


    writeXml( findSubNode(typeNode, "lineMaxSpeed", ok), coordinateConfigIn.lineMaxSpeed  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: lineMaxSpeed";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5207, robotId);
        return -1;
    }



    writeXml( findSubNode(typeNode, "lineMaxAcceleration", ok), coordinateConfigIn.lineMaxAcceleration  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: lineMaxAcceleration";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5208, robotId);
        return -1;
    }


    writeXml( findSubNode(typeNode, "rotate_eqradius", ok), coordinateConfigIn.rotate_eqradius  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: rotate_eqradius";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5202, robotId,196);
        return -1;
    }

    writeXml( findSubNode(typeNode, "jogDecartRotateRation", ok), coordinateConfigIn.jogDecartRotateRation  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: jogDecartRotateRation";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5202, robotId,196);
        return -1;
    }

    writeXml( findSubNode(typeNode, "jogDecartLineRatio", ok), coordinateConfigIn.jogDecartLineRatio  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: jogDecartLineRatio";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5202, robotId,196);
        return -1;
    }

    closeXml(true);

    arm_CeaseTime=coordinateConfigIn.arm_CeaseTime;
    arm_StopTime=coordinateConfigIn.arm_StopTime;
    lineMaxAcceleration=coordinateConfigIn.lineMaxAcceleration;
    lineMaxSpeed=coordinateConfigIn.lineMaxSpeed;
    rotate_eqradius=coordinateConfigIn.rotate_eqradius;
    t1RatioLimit=coordinateConfigIn.t1RatioLimit;
    targetWaitTime=coordinateConfigIn.targetWaitTime;
    jogDecartLineRatio=coordinateConfigIn.jogDecartLineRatio;
    jogDecartRotateRation=coordinateConfigIn.jogDecartRotateRation;


    return 1;

}


int RobotAttribute::getRobotAttribute(RobotCommonParameter & roboParameter)
{
    QString readComment;
    int returnValue=readFile(readComment);
    if(-1==returnValue)
    {
        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5228, robotId
               ,0,0,readComment);
        return -1;
    }

    #ifdef D_USE_MD5_CHECK
    returnValue=checkFile();
    if(-1==returnValue)
    {
        return -1;
    }
    #endif

    roboParameter.coordinateSize=coordinateSize;
    roboParameter.fileName=filePath;
    roboParameter.armControlAxisNumber=armControlAxisNumber;
    roboParameter.externalNum= externalNum;
    roboParameter.jointNum=jointNum;
    roboParameter.agvAxisNum=agvAxisNum;
    roboParameter.virtualAxisNum=virtualAxisNum;
    roboParameter.robotId=robotId;
    roboParameter.jointAttributeVector=robotJoints;
    roboParameter.robotType=robotType;
    roboParameter.t1RatioLimit=t1RatioLimit;
    roboParameter.arm_CeaseTime=arm_CeaseTime;
    roboParameter.arm_StopTime=arm_StopTime;
    roboParameter.arm_warnSpeedRatio=arm_warnSpeedRatio;
    roboParameter.arm_warnSlowRatio=arm_warnSlowRatio;
//    roboParameter.agv_CeaseTime=agv_CeaseTime;
//    roboParameter.agv_StopTime=agv_StopTime;
    roboParameter.deltaTime=deltaTime;
    roboParameter.targetWaitTime=targetWaitTime;
    roboParameter.lineMaxSpeed=lineMaxSpeed;
    roboParameter.lineMaxAcceleration=lineMaxAcceleration;
    roboParameter.jointMoveVelocityType=(E_VELOCITY_PROFILE_TYPE)jointMoveVelocityType;
    roboParameter.agv_targetWaitTime=agv_targetWaitTime;
    roboParameter.agv_lineMaxSpeed=agv_lineMaxSpeed;
    roboParameter.agv_lineMaxAcceleration=agv_lineMaxAcceleration;
    roboParameter.agv_rotateMaxSpeed=agv_rotateMaxSpeed;
    roboParameter.agv_rotateMaxAcceleration=agv_rotateMaxAcceleration;
    roboParameter.agv_jointMoveVelocityType=(E_VELOCITY_PROFILE_TYPE)agv_jointMoveVelocityType;
    roboParameter.agv_allowAngleOffset=agv_allowAngleOffset;
    roboParameter.agv_allowLineOffset=agv_allowLineOffset;
    roboParameter.agv_allowAngleMovingOffset=agv_allowAngleMovingOffset;
    roboParameter.agv_allowLineMovingOffset=agv_allowLineMovingOffset;
    roboParameter.agv_allowLineMovingOffset_ratio=agv_allowLineMovingOffset_ratio;
    roboParameter.agv_emergencyRotateAcceleration=agv_emergencyRotateAcceleration;
    roboParameter.agv_emergencyLineAcceleration=agv_emergencyLineAcceleration;
    roboParameter.allowVelFeedbackBiasRatio=allowVelFeedbackBiasRatio;

    //直线运动
    roboParameter.coordinateAttribute.resize(1);
//    roboParameter.coordinateAttribute[0]=robotJoints[0];
    roboParameter.coordinateAttribute[0].maxVelocity=lineMaxSpeed;
    roboParameter.coordinateAttribute[0].maxAccelation=lineMaxAcceleration;

    roboParameter.isArmMotionOn=isArmMotionOn;
    roboParameter.isAgvMotionOn=isAgvMotionOn;

    roboParameter.rotate_eqradius=rotate_eqradius;
    roboParameter.gravity=gravity;
    roboParameter.isCalculateIdealJointTorque=isCalculateIdealJointTorque;
    roboParameter.armTpType=armTpType;
    roboParameter.maxLineCatshDepth_carvingTp=maxLineCatshDepth_carvingTp;

    roboParameter.jogDecartLineRatio=jogDecartLineRatio;
    roboParameter.jogDecartRotateRation=jogDecartRotateRation;


    return returnValue;
}

int RobotAttribute::getMotionServerModel(QString fileNameIn, MC_MOTION_MODEL &modelOut)
{
    QVariant strConfigValue;
    QString firstTimeFlagStr = fileNameIn;
    QString strConfigGroup = "";
    QString strConfigKey = "";

    QSettings settings(firstTimeFlagStr,QSettings::IniFormat);
    settings.setIniCodec("UTF-8");
    strConfigGroup = "ControlInfo";
    settings.beginGroup(strConfigGroup);

    //modelOut
    strConfigKey = "motionMode";
    if(false==settings.contains(strConfigKey))
    {
        return -1;
    }
    strConfigValue = settings.value(strConfigKey);
    modelOut= (MC_MOTION_MODEL)strConfigValue.toInt();


    settings.endGroup();

    qDebug() << "RobotAttribute::getMotionServerModel sucess";
    return 1;
}

int RobotAttribute::getAutoRatio(QString fileNameIn,double &autoRatioOut,double &manualRatioOut,int &recordWeaveOut)
{
    qDebug()<<"getAutoRatio,fileName="<<fileNameIn;
    QSettings settings(fileNameIn, QSettings::IniFormat);

    QString strConfigKey;

    strConfigKey= "common/autoRatio";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -1;
    }
    autoRatioOut = settings.value(strConfigKey).toDouble();

    strConfigKey= "common/jogRatio";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -2;
    }
    manualRatioOut = settings.value(strConfigKey).toDouble();

    strConfigKey= "common/recordWeave";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter recordWeave";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -2;
    }
    recordWeaveOut = settings.value(strConfigKey).toInt();

    return 1;

}



int RobotAttribute::writeAutoRatio(QString fileNameIn, double autoRatioIn, double manualRatioIn)
{
    qDebug()<<"writeAutoRatio,fileName="<<fileNameIn;
    QSettings settings(fileNameIn, QSettings::IniFormat);

    settings.setValue("common/autoRatio",QVariant::fromValue(autoRatioIn));
    settings.setValue("common/jogRatio",QVariant::fromValue(manualRatioIn));

    return 1;
}

int RobotAttribute::getLastFloorInfo(QString fileNameIn, PathMovingInfo &pathMovingOut, VehiclePosition &positionOut)
{
    qDebug()<<"RobotAttribute::getLastFloorInfo,fileName="<<fileNameIn;
    QSettings settings(fileNameIn, QSettings::IniFormat);

    QString strConfigKey;

    strConfigKey= "floorPosition/passedStationId";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -1;
    }
    pathMovingOut.passedStationId = settings.value(strConfigKey).toInt();

    strConfigKey= "floorPosition/nextStationId";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -1;
    }
    pathMovingOut.nextStationId = settings.value(strConfigKey).toInt();


    strConfigKey= "floorPosition/currentStationId";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -1;
    }
    pathMovingOut.currentStationId = settings.value(strConfigKey).toInt();


    strConfigKey= "floorPosition/passedPathId";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -1;
    }
    pathMovingOut.passedPathId = settings.value(strConfigKey).toInt();


    strConfigKey= "floorPosition/nextPathId";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -1;
    }
    pathMovingOut.nextPathId = settings.value(strConfigKey).toInt();


    strConfigKey= "floorPosition/currentPathId";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -1;
    }
    pathMovingOut.currentPathId = settings.value(strConfigKey).toInt();


    strConfigKey= "floorPosition/currentFloorId";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -1;
    }
    pathMovingOut.currentFloorId = settings.value(strConfigKey).toInt();

    strConfigKey= "floorPosition/position_x";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -1;
    }
    positionOut.x = settings.value(strConfigKey).toDouble();

    strConfigKey= "floorPosition/position_y";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -1;
    }
    positionOut.y = settings.value(strConfigKey).toDouble();

    strConfigKey= "floorPosition/position_rotate";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "MotionServer", "RobotAttribute" , 5220, robotId);
        return -1;
    }
    positionOut.rotate = settings.value(strConfigKey).toDouble();


    qDebug()<<"RobotAttribute::getLastFloorInfo,positionOut"<<positionOut.x <<positionOut.y<<positionOut.rotate;
    return 1;
}

int RobotAttribute::writeLastFloorInfo(QString fileNameIn, PathMovingInfo pathMovingIn, VehiclePosition positionIn)
{
//    qDebug()<<"writeLastFloorInfo,fileName="<<fileNameIn;
    QSettings settings(fileNameIn, QSettings::IniFormat);

    settings.setValue("floorPosition/currentFloorId",QVariant::fromValue(pathMovingIn.currentFloorId));
    settings.setValue("floorPosition/passedStationId",QVariant::fromValue(pathMovingIn.passedStationId));
    settings.setValue("floorPosition/nextStationId",QVariant::fromValue(pathMovingIn.nextStationId));
    settings.setValue("floorPosition/currentStationId",QVariant::fromValue(pathMovingIn.currentStationId));
    settings.setValue("floorPosition/passedPathId",QVariant::fromValue(pathMovingIn.passedPathId));
    settings.setValue("floorPosition/nextPathId",QVariant::fromValue(pathMovingIn.nextPathId));
    settings.setValue("floorPosition/currentPathId",QVariant::fromValue(pathMovingIn.currentPathId));
    if(isnan(positionIn.x) || isnan(positionIn.y)
             ||isnan(positionIn.rotate))
    {
        qDebug()<<"warn:writeLastFloorInfo,isnan(positionIn.x) || isnan(positionIn.y)||isnan(positionIn.rotate)";
    }
    else
    {
        settings.setValue("floorPosition/position_x",QVariant::fromValue(positionIn.x));
        settings.setValue("floorPosition/position_y",QVariant::fromValue(positionIn.y));
        settings.setValue("floorPosition/position_rotate",QVariant::fromValue(positionIn.rotate));

    }

    return 1;
}

int RobotAttribute::isUsePositionRegulator()
{
    return isUsePositionRegulatorStatus;
}

int RobotAttribute::getAutoSaveWaveFlag()
{
    return autoSaveWaveFlag;
}

int RobotAttribute::checkFile()
{
    MD5Check md5Checker;
    if(-1==md5Checker.isMd5CheckOk(filePath,filePath+".md5"))
    {
        if(IF_DEBUG)
        {
            qDebug()<<"RobotAttribute::checkFile() error！";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5218, robotId);

        return -1;
    }
}

int RobotAttribute::readFile( QString &readComent)
{
    bool ok = openXml(filePath, QIODevice::ReadOnly );
    if( !ok )//无法打开机器人数据文件！
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====无法打开机器人参数文件！";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5200, robotId);

        return -1;
    }
    int robotAxisesCount=0;
    int extAxisesCount =0;
    QDomNode  tmpNode;

    //读机器人类型 T1限速值 坐标系数量
    int tmpRobotType;
    double tmpT1Ratio;
    QDomNode  typeNode = findSubNode(m_root, "robotSetting", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotSetting"<<filePath;
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5201, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "robotType", ok), tmpRobotType  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotType";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5202, robotId);
        return -1;
    }
    robotType = tmpRobotType;
    if(ENUM_ROBOT_TYPE_END <= robotType ||1>robotType)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotType不支持"<<robotType;
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5203, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "t1RatioLimit", ok), tmpT1Ratio  );
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取T1限速失败，使用默认值";
        }
        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5204, robotId);
        return -1;
    }
    else if(ok && ( ( tmpT1Ratio<0)||(tmpT1Ratio>1) ) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取T1限速失败，tmpT1Ratio<0 || tmpT1Ratio>1";
        }
        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5205, robotId);
    }
    else
    {
        t1RatioLimit = tmpT1Ratio;

    }

    readXml( findSubNode(typeNode, "isCalculateIdealJointTorque", ok), isCalculateIdealJointTorque);
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取isCalculateIdealJointTorque失败";
        }
        readComent+=" isCalculateIdealJointTorque";
//        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5226, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "armTpType", ok), armTpType);
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取armTpType失败";
        }
        readComent+=" armTpType";
//        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5226, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "maxLineCatshDepth_carvingTp", ok), maxLineCatshDepth_carvingTp);
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取maxLineCatshDepth_carvingTp失败";
        }
        readComent+=" maxLineCatshDepth_carvingTp";
//        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5226, robotId);
        return -1;
    }

    gravity.resize(3);
    readXml( findSubNode(typeNode, "gravity_x", ok), gravity[0]);
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取gravity_x失败";
        }
         readComent+=" gravity_x";
//        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5225, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "gravity_y", ok), gravity[1]);
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取gravity_y失败";
        }
         readComent+=" gravity_y";
//        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5225, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "gravity_z", ok), gravity[2]);
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取gravity_z失败";
        }
         readComent+=" gravity_z";
//        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5225, robotId);
        return -1;
    }


    readXml( findSubNode(typeNode, "arm_CeaseTime", ok), arm_CeaseTime  );
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取arm_CeaseTime失败";
        }
         readComent+=" arm_CeaseTime";
//        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5223, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "arm_StopTime", ok), arm_StopTime  );
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取arm_StopTime失败";
        }
         readComent+=" arm_StopTime";
//        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5224, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "arm_warnSpeedRatio", ok), arm_warnSpeedRatio  );
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取arm_warnSpeedRatio失败";
        }
         readComent+=" arm_warnSpeedRatio";
//        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5227, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "arm_warnSlowRatio", ok), arm_warnSlowRatio  );
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取arm_warnSlowRatio失败";
        }
         readComent+=" arm_warnSlowRatio";
//        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "McControllerInterpreter", 5227, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "targetWaitTime", ok), targetWaitTime  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: targetWaitTime";
        }
         readComent+=" targetWaitTime";
//        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5206, robotId);
        return -1;
    }
    readXml( findSubNode(typeNode, "isUsePositionRegulatorStatus", ok), isUsePositionRegulatorStatus  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: isUsePositionRegulatorStatus";
        }
        readComent+=" isUsePositionRegulatorStatus";
//        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5221, robotId);
        return -1;
    }
    readXml( findSubNode(typeNode, "autoSaveWaveFlag", ok), autoSaveWaveFlag  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: autoSaveWaveFlag";
        }
         readComent+=" autoSaveWaveFlag";
//        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5222, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_targetWaitTime", ok), agv_targetWaitTime  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_targetWaitTime";
        }
         readComent+=" agv_targetWaitTime";
//        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5206, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "lineMaxSpeed", ok), lineMaxSpeed  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: lineMaxSpeed";
        }
         readComent+=" lineMaxSpeed";
//        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5207, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_lineMaxSpeed", ok), agv_lineMaxSpeed  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_lineMaxSpeed";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5207, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_rotateMaxSpeed", ok), agv_rotateMaxSpeed  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_rotateMaxSpeed";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5207, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "lineMaxAcceleration", ok), lineMaxAcceleration  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: lineMaxAcceleration";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5208, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_lineMaxAcceleration", ok), agv_lineMaxAcceleration  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_lineMaxAcceleration";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5208, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_rotateMaxAcceleration", ok), agv_rotateMaxAcceleration  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_rotateMaxAcceleration";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5208, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_emergencyRotateAcceleration", ok), agv_emergencyRotateAcceleration  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_emergencyRotateAcceleration";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5208, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_emergencyLineAcceleration", ok), agv_emergencyLineAcceleration  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_emergencyLineAcceleration";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5208, robotId);
        return -1;
    }

    readXml( findSubNode(typeNode, "jointMoveVelocityType", ok), jointMoveVelocityType  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: jointMoveVelocityType";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5219, robotId,205);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_jointMoveVelocityType", ok), agv_jointMoveVelocityType  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_jointMoveVelocityType";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5219, robotId,204);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_allowLineOffset", ok), agv_allowLineOffset  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_allowLineOffset";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5219, robotId,203);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_allowAngleOffset", ok), agv_allowAngleOffset  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_allowAngleOffset";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5219, robotId,202);
        return -1;
    }
    readXml( findSubNode(typeNode, "agv_allowLineMovingOffset", ok), agv_allowLineMovingOffset  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_allowLineMovingOffset";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5219, robotId,201);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_allowLineMovingOffset_ratio", ok), agv_allowLineMovingOffset_ratio  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_allowLineMovingOffset_ratio";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5219, robotId,201);
        return -1;
    }

    readXml( findSubNode(typeNode, "agv_allowAngleMovingOffset", ok), agv_allowAngleMovingOffset  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: agv_allowAngleMovingOffset";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5219, robotId,200);
        return -1;
    }
    readXml( findSubNode(typeNode, "rotate_eqradius", ok), rotate_eqradius  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: rotate_eqradius";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5202, robotId,196);
        return -1;
    }
    readXml( findSubNode(typeNode, "allowVelFeedbackBiasRatio", ok), allowVelFeedbackBiasRatio  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: allowVelFeedbackBiasRatio";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5202, robotId,196);
        return -1;
    }

    readXml( findSubNode(typeNode, "jogDecartRotateRation", ok), jogDecartRotateRation  );
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取jogDecartRotateRation失败";
        }
         readComent+=" jogDecartRotateRation";
         return -1;
    }

    readXml( findSubNode(typeNode, "jogDecartLineRatio", ok), jogDecartLineRatio  );
    if( (!ok) )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读取jogDecartLineRatio失败";
        }
         readComent+=" jogDecartRotateRation";
         return -1;

    }

    //--------------------------
    typeNode = findSubNode(m_root, "ModelSetting", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotSetting"<<filePath;
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5219, robotId,199);
        return -1;
    }

    readXml( findSubNode(typeNode, "isArmMotionOn", ok), isArmMotionOn  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotType";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5202, robotId,198);
        return -1;
    }
    readXml( findSubNode(typeNode, "isAgvMotionOn", ok), isAgvMotionOn  );
    if( !ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotType";
        }
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5202, robotId,197);
        return -1;
    }


    ///读内部轴参数
    QDomNode  inNode = findSubNode(m_root, "robotAxis", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotAxis"<<filePath;
        }
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","McControllerInterpreter",5209,robotId);
        return -1;
    }

    readXml( findSubNode(inNode, "count", ok), robotAxisesCount  );
    if(!ok)
    {
        if(IF_DEBUG)
        {
            qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: robotAxis count"<<robotAxisesCount<<filePath;
        }
        addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","McControllerInterpreter",5210,robotId);
    }
    robotJoints.clear();

    QVector<bool> okReturn;
    JointAttribute joint;
    for(int i=0;i<robotAxisesCount;i++)
    {
         tmpNode =  findSubNode(inNode, QString("axis%1").arg(i), ok);
         if( !ok  )
         {
            if(IF_DEBUG)
            {
                qDebug()<<"McControllerInterpreter::readRobotInfo====读内部轴参数失败: axis "<<i<<filePath;
            }
            addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","McControllerInterpreter",5211,robotId,i);
            return -1;
         }

         joint.externAxisFlag = 0;
         readAxisParameter(okReturn,tmpNode,joint);
         robotJoints.append(joint);

        for(int k=0;k<okReturn.size();k++)
        {
           if( !okReturn[k]  )
           {
              if(IF_DEBUG)
              {
                  qDebug()<<"McControllerInterpreter::readRobotInfo====读内部轴参数节点失败: axis "<<i<<"axisParameter"<<k;
              }
              addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","McControllerInterpreter",5212,robotId,i,k);
              return -1;
           }
        }
    }

     ///读外部轴参数
//     QDomNode extNode = findSubNode(m_root, "extAxis", ok );
//     if( !ok )
//     {
//         if(IF_DEBUG)
//         {
//             qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: extAxis"<<fileName;
//         }

//         addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","McControllerInterpreter",5213,robotId);
//         return -1;
//     }
//     readXml( findSubNode(extNode, "count", ok), extAxisesCount  );
//     if(!ok )
//     {
//         if(IF_DEBUG)
//         {
//             qDebug()<<"McControllerInterpreter::readRobotInfo====读文件失败: extAxis  count"<<extAxisesCount<<fileName;
//         }

//         addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","McControllerInterpreter",5214,robotId);
//         return -1;
//     }

//     for(int i=0;i<extAxisesCount;i++)
//     {
//         tmpNode =  findSubNode(extNode, QString("axis%1").arg(i), ok);
//         if( !ok  )
//         {
//             if(IF_DEBUG)
//             {
//                qDebug()<<"McControllerInterpreter::readRobotInfo====读外部轴参数失败: axis "<<i<<fileName;
//             }
//             addMsg(ENUM_MSG_ERROR,"McControllerInterpreter", "McControllerInterpreter", 5215, robotId);
//             return -1;
//         }


//         joint.externAxisFlag = 1;
//         readAxisParameter(okReturn,tmpNode,joint);
//         robotJoints.append(joint);

//           for(int k=0;k<okReturn.size();k++)
//           {
//               if( !okReturn[k]  )
//               {
//                  if(IF_DEBUG)
//                  {
//                      qDebug()<<"McControllerInterpreter::readRobotInfo====读外部轴参数节点失败: axis "<<i<<"axisParameter"<<k;
//                  }
//                  addMsg(ENUM_MSG_ERROR,"McControllerInterpreter","McControllerInterpreter",5212,robotId,i,k);
//                  return -1;
//               }
//           }
//    }

     closeXml( );

    jointNum = robotJoints.size();//读完机械臂配置后，获取关节数目
    armControlAxisNumber=0;
    externalNum=0;
    agvAxisNum=0;
    virtualAxisNum=0;
    for(int i=0;i<jointNum;i++)
    {
        if(E_AXIS_LOGIC_TYPE_COMMON==robotJoints[i].axisLogicalType)
        {
            armControlAxisNumber ++;
        }
        else if(E_AXIS_LOGIC_TYPE_EXT==robotJoints[i].axisLogicalType)
        {
            externalNum ++;
        }
        else if(E_AXIS_LOGIC_TYPE_AGV==robotJoints[i].axisLogicalType)
        {
            agvAxisNum ++;
            armControlAxisNumber ++;
        }
        else if(E_AXIS_LOGIC_TYPE_AGVSTEERING==robotJoints[i].axisLogicalType)
        {
            agvAxisNum ++;
            armControlAxisNumber ++;
        }
        else if(E_AXIS_LOGIC_TYPE_VIRTUAL_ROBOT==robotJoints[i].axisLogicalType)
        {
            virtualAxisNum ++;
        }
        else if(E_AXIS_LOGIC_TYPE_VIRTUAL_PARALLEL==robotJoints[i].axisLogicalType)
        {
            virtualAxisNum ++;
        }
        else if(E_AXIS_LOGIC_TYPE_VIRTUAL_CAM==robotJoints[i].axisLogicalType)
        {
            virtualAxisNum ++;
        }
        else
        {
           virtualAxisNum ++;
        }

    }

//    if (internalNum==0)
//    {
//        if(IF_DEBUG)
//        {
//            qDebug()<< "McControllerInterpreter::readRobotInfo====error: 内部轴数量为0";
//        }
//        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "McControllerInterpreter", 5217, robotId);
//        return -1;
//    }
//    externalNum = extAxisesCount;
    return 1;
}



int RobotAttribute::readAxisParameter(QVector<bool> &okReturn,QDomNode  tmpNode,JointAttribute &joint)
{

    joint.massCenter.resize(3);
    joint.inertiaMatrix.resize(6);
    okReturn.resize(46);

    joint.t1RatioLimit=t1RatioLimit;

    readXml( findSubNode(tmpNode, "jointType", okReturn[0]),   joint.jointType  );
    readXml( findSubNode(tmpNode, "targetFollowError", okReturn[1]), joint.targetFollowError );
    readXml( findSubNode(tmpNode, "dhType", okReturn[2]),   joint.dhType  );
    readXml( findSubNode(tmpNode, "mass", okReturn[3]),   joint.mass  );
    readXml( findSubNode(tmpNode, "coupleFlag", okReturn[4]),   (unsigned int&)joint.coupleFlag  );

    readXml( findSubNode(tmpNode, "maxPosition", okReturn[5]),  joint.maxPosition  );
    readXml( findSubNode(tmpNode, "minPosition", okReturn[6]),  joint.minPosition  );

    readXml( findSubNode(tmpNode, "dhParameter0", okReturn[7]),  joint.dhParameter[0]  );
    readXml( findSubNode(tmpNode, "dhParameter1", okReturn[8]),  joint.dhParameter[1]  );
    readXml( findSubNode(tmpNode, "dhParameter2", okReturn[9]),  joint.dhParameter[2]  );
    readXml( findSubNode(tmpNode, "dhParameter3", okReturn[10]),  joint.dhParameter[3]  );

    readXml( findSubNode(tmpNode, "massCenter0", okReturn[11]),  joint.massCenter[0]  );
    readXml( findSubNode(tmpNode, "massCenter1", okReturn[12]),  joint.massCenter[1]  );
    readXml( findSubNode(tmpNode, "massCenter2", okReturn[13]),  joint.massCenter[2]  );

    readXml( findSubNode(tmpNode, "inertiaMatrix0", okReturn[14]),  joint.inertiaMatrix[0]  );
    readXml( findSubNode(tmpNode, "inertiaMatrix1", okReturn[15]),  joint.inertiaMatrix[1]  );
    readXml( findSubNode(tmpNode, "inertiaMatrix2", okReturn[16]),  joint.inertiaMatrix[2]  );
    readXml( findSubNode(tmpNode, "inertiaMatrix3", okReturn[17]),  joint.inertiaMatrix[3]  );
    readXml( findSubNode(tmpNode, "inertiaMatrix4", okReturn[18]),  joint.inertiaMatrix[4]  );
    readXml( findSubNode(tmpNode, "inertiaMatrix5", okReturn[19]),  joint.inertiaMatrix[5]  );

    readXml( findSubNode(tmpNode, "maxVelocity", okReturn[20]),  joint.maxVelocity );
    readXml( findSubNode(tmpNode, "maxAccelation", okReturn[21]),  joint.maxAccelation );

    QString tmpStr="";
    readXml( findSubNode(tmpNode, "jointName", okReturn[22]), tmpStr );
    joint.jointName=tmpStr.toStdString();

     readXml( findSubNode(tmpNode, "positionOffset", okReturn[23]),  joint.positionOffset );
     readXml( findSubNode(tmpNode, "maxTorque", okReturn[24]),  joint.maxTorque );
     readXml( findSubNode(tmpNode, "motorRatedTorque", okReturn[25]),  joint.motorRatedTorque );
     readXml( findSubNode(tmpNode, "viscousFriction", okReturn[26]),  joint.viscousFriction );
     readXml( findSubNode(tmpNode, "coulombFriction", okReturn[27]),  joint.coulombFriction );
     readXml( findSubNode(tmpNode, "mobile", okReturn[28]),  (unsigned int&)joint.mobile );
     readXml( findSubNode(tmpNode, "gearRatio", okReturn[29]),  joint.gearRatio );
     readXml( findSubNode(tmpNode, "screwPitch", okReturn[30]),  joint.screwPitch );
     readXml( findSubNode(tmpNode, "pulsePerRound", okReturn[31]),  joint.pulsePerRound );
     readXml( findSubNode(tmpNode, "motorDirection", okReturn[32]),  joint.motorDirection );
     readXml( findSubNode(tmpNode, "processFollowError", okReturn[33]),  joint.processFollowError );
     readXml( findSubNode(tmpNode, "coupleFlag", okReturn[34]),   (unsigned int&)joint.coupleFlag );
     readXml( findSubNode(tmpNode, "coupleJoint", okReturn[35]),  joint.coupleJoint );
     readXml( findSubNode(tmpNode, "coupleReducer", okReturn[36]),  joint.coupleReducer );
     readXml( findSubNode(tmpNode, "externAxisFlag", okReturn[37]),  joint.externAxisFlag );
     readXml( findSubNode(tmpNode, "jogTorqueLimit", okReturn[38]),  joint.jogTorqueLimit );
     readXml( findSubNode(tmpNode, "motorTorqueLimitPositive", okReturn[39]),  joint.motorTorqueLimitPositive );
     readXml( findSubNode(tmpNode, "motorTorqueLimitNegative", okReturn[40]),  joint.motorTorqueLimitNegative );
     readXml( findSubNode(tmpNode, "motorTorqueFilterTime", okReturn[41]),  joint.motorTorqueFilterTime );
     int tmp_type;
     readXml( findSubNode(tmpNode, "axisLogicalType", okReturn[42]), tmp_type  );
     joint.axisLogicalType=(E_AXIS_LOGIC_TYPE)tmp_type;
     readXml( findSubNode(tmpNode, "processVelocityFollowError", okReturn[43]), joint.processVelocityFollowError  );
     readXml( findSubNode(tmpNode, "maxFeedbackVelocity", okReturn[44]),  joint.maxFeedbackVelocity );
     readXml( findSubNode(tmpNode, "approachDistance", okReturn[45]),  joint.approachDistance  );


     return 1;

}

void RobotAttribute::addMsg(int messageLevel, std::string componentName, std::string messageType
                            , int messageCode, int _robotId, int parameter1, int parameter2, QString comment)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = _robotId;

    string infomationStr;

    switch(messageCode)
    {
    case 5228://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml读取失败！").toStdString()+comment.toStdString();
        infomationStr = str1;
        break;
    }
    case 5227://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到arm_warnSpeedRatio arm_warnSlowRatio节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5226://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到isCalculateIdealJointTorque节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5225://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到gravity_节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5224://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到arm_StopTime节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5223://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到arm_CeaseTime节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5222://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到autoSaveWaveFlag节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5221://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到isUsePositionRegulatorStatus节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5220://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动runConfig.ini文件缺少参数！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5219://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml出错，参数id=！").toStdString()+
                QString::number(parameter1).toStdString();
        infomationStr = str1;
        break;
    }
    case 5218://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml校验出错，请联系设备供应商！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5217://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml没有轴的数据！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5216:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(_robotId).toStdString() +
               QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml,extAxis  axis").toStdString() +
               QString::number(parameter1+1).toStdString() +
               QObject::tr("缺少参数:").toStdString()+
               QString::number(parameter2+1).toStdString();
       infomationStr = str1;
       break;
    }
    case 5215:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(_robotId).toStdString() +
               QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml,extAxis找不到 axis").toStdString() +
               QString::number(parameter1+1).toStdString() +
               QObject::tr("节点！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5214://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml,extAxis找不到count节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5213://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml,找不到extAxis节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5212:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(_robotId).toStdString() +
               QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml,  axis").toStdString() +
               QString::number(parameter1+1).toStdString() +
               QObject::tr("缺少参数:").toStdString()+
               QString::number(parameter2+1).toStdString();
       infomationStr = str1;
       break;
    }
    case 5211:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(_robotId).toStdString() +
               QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml,找不到 axis").toStdString() +
               QString::number(parameter1+1).toStdString() +
               QObject::tr("节点！").toStdString();
       infomationStr = str1;
       break;
    }
    case 5210://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml,找不到count节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5209://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到robotAxis节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5208://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到lineMaxAcceleration节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5207://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到lineMaxSpeed节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5206://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到targetWaitTime节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5205://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml,t1RatioLimit设置范围错误！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5204://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到t1RatioLimit节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5203://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml,robotType设置范围错误！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5202://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到robotType节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5201://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")机器人运动参数配置文件mcControllerInterpreter.xml找不到robotSetting节点！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5200://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")打开机器人运动参数配置文件mcControllerInterpreter.xml失败！").toStdString();
        infomationStr = str1;
        break;
    }


        default:
        {
            qDebug()<<"Coordinate::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}
